#include <general_interface/msg/control_signal.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>

/**
 * @brief KinnematicRobot Node
 *        This node is used to simulate the kinematic robot
 */
class KinnematicRobot : public rclcpp::Node
{
  private:
    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odomPub;
    rclcpp::Subscription<general_interface::msg::ControlSignal>::SharedPtr
        controlSub;
    rclcpp::TimerBase::SharedPtr timer;
    double x = 0, y = 0, z = 0;
    double yaw = 0, pitch = 0, roll = 0;
    double gyro = 0;
    double speed = 0, acc = 0;
    // factor = l/g = 0.05/9.8=0.0051
    double factor = 0.0051;
    const static int SimRate = 100;

    double maxSpeed = 0;

  public:
    KinnematicRobot() : Node("kinnematic_robot")
    {
        RCLCPP_INFO(this->get_logger(), "KinnematicRobot Node Started");

        declare_parameter("factor", factor);
        get_parameter("factor", factor);

        declare_parameter("max_speed", 5.0);
        get_parameter("max_speed", maxSpeed);

        odomPub =
            this->create_publisher<nav_msgs::msg::Odometry>("/sp_odom", 10);
        controlSub =
            this->create_subscription<general_interface::msg::ControlSignal>(
                "/control_signal", 10,
                [this](
                    const general_interface::msg::ControlSignal::SharedPtr msg)
                {
                    pitch = msg->forward_control;
                    gyro = msg->rotate_control;
                });
        timer = this->create_wall_timer(
            std::chrono::milliseconds(1000 / SimRate),
            [this]()
            {
                double dt = 1.0 / SimRate;
                acc = tan(pitch) * factor;
                yaw += gyro * dt;
                speed += factor * dt;
                speed = std::clamp(speed, -maxSpeed, maxSpeed);
                x += speed * cos(yaw) * dt;
                y += speed * sin(yaw) * dt;

                PublishOdometry();
            });
    }

    void PublishOdometry()
    {
        nav_msgs::msg::Odometry odom;
        odom.header.stamp = this->get_clock()->now();
        odom.header.frame_id = "map";
        odom.child_frame_id = "footprint";
        odom.pose.pose.position.x = x;
        odom.pose.pose.position.y = y;
        odom.pose.pose.position.z = z;
        tf2::Quaternion q;
        q.setRPY(roll, pitch, yaw);
        odom.pose.pose.orientation.x = q.x();
        odom.pose.pose.orientation.y = q.y();
        odom.pose.pose.orientation.z = q.z();
        odom.pose.pose.orientation.w = q.w();
        odom.twist.twist.linear.x = speed;
        odom.twist.twist.linear.y = 0;
        odom.twist.twist.linear.z = 0;
        odom.twist.twist.angular.x = 0;
        odom.twist.twist.angular.y = 0;
        odom.twist.twist.angular.z = gyro;
        odomPub->publish(odom);
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<KinnematicRobot>());
    rclcpp::shutdown();
    return 0;
}